
function PseudoArc(xj_guess , aux)
%
% 伪弧长延拓，输出脉冲最优解
%
% 20220516
% 张晨
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

warning off;

% disp_IO = 0; % 不显示过程
disp_IO = 1; % 显示过程

% 载入伪弧长参数
pseudoArc = aux.pseudoArc;

% 初始化 xj 和 dir_j
xj = xj_guess; % 优化变量

% 伪弧长方向
[c, ceq, Gc, Gceq] = Bcr4bp_ConsEMRot2D(xj_guess , aux);
Gceq = Gceq';

% 零空间向量
if strcmp(pseudoArc.dir , 'dir1')
    dir_j = null(Gceq);
elseif strcmp(pseudoArc.dir , 'dir2')
    dir_j = - null(Gceq);
end

% 错误标签初始化
err_flag = 0;

result = [];
result.tof = [];
result.dv_t0 = [];
result.dv_tf = [];
result.thetaS_t0 = [];
result.thetaS_tf = [];
result.sigma = [];

%% 伪弧长延拓循环
for iLoop = 1 : pseudoArc.maxNum

    % 步骤1：预测
    xjp1 = xj + dir_j * pseudoArc.deltas;

    % 步骤2：校正
    err = inf;
    count = 1;
    iterMax = 20;
    while err > 1e-8 && count < iterMax

        % 计算误差和梯度矩阵
        [c, ceq, Gc, Gceq] = Bcr4bp_ConsEMRot2D(xjp1 , aux);
        Gceq = Gceq';

        % 计算误差
        err = max(abs(ceq));

        % 更新xjp1
        % xjp1 = xjp1 - inv(Gceq) * ceq;
        xjp1 = xjp1 - Gceq \ ceq;

        % 计数器更新
        count = count + 1;

    end

    % 显示迭代过程
    if disp_IO == 1
        fprintf('迭代数: %0.0f, 误差: %0.2e \n' , iLoop , err)
    end

    % 计算次数错误
    if count == iterMax
        err_flag = 1;
    end

    % 更新 xj 和 dir_j
    xj = xjp1;
    dir_jp1 = null(Gceq);

    % 判断延拓方向
    if dot(dir_j , dir_jp1) < 0
        dir_j = - dir_jp1;
    else
        dir_j = dir_jp1;
    end

    % 画伪弧长延拓后的轨道
    PlotPseudoArc(xjp1 , aux);

    % 保存伪弧长延拓结果
    resultTemp = getResult(xjp1 , aux);
    result.tof = [result.tof ; resultTemp.tof];
    result.dv_t0 = [result.dv_t0 ; resultTemp.dv_t0];
    result.dv_tf = [result.dv_tf ; resultTemp.dv_tf];
    result.thetaS_t0 = [result.thetaS_t0 ; resultTemp.thetaS_t0];
    result.thetaS_tf = [result.thetaS_tf ; resultTemp.thetaS_tf];
    result.sigma = [result.sigma ; resultTemp.sigma];

end

figure(4); hold on; grid on;
plot(result.thetaS_t0 * 180 / pi , result.dv_t0 , 'b')


end
